#include "usart.h"
#include "main.h"
#include "rps.h"
#include "cmsis_os.h"

float pos_x=0;
float pos_y=0;
float zangle=0;
float xangle=0;
float yangle=0;
float w_z=0;
float count=0;
float data_count=0;

float RunTimeTimer = 0;

void SetRedLEDStatus(float time){
	if(HAL_GetTick() - RunTimeTimer>time)
	{
		HAL_GPIO_TogglePin(RLED_GPIO_Port,RLED_Pin);
		RunTimeTimer = HAL_GetTick();
	}

}

void receive(){
	 static uint8_t ch;
		static union
		{
			uint8_t data[24];
			float ActVal[6];
		}posture;//¶¨ҥʽ¾ݴ󐡺͊ýשûז
		static uint8_t i=0;
		static uint8_t data_count=0;
	//	__HAL_UART_ENABLE_IT(&huart2, UART_IT_RXNE);
	if (__HAL_UART_GET_FLAG(&huart4,UART_FLAG_IDLE) != RESET)
			__HAL_UART_CLEAR_IDLEFLAG(&huart4);
	if(huart4.Instance->SR& UART_FLAG_RXNE)//testing the SR for IT
	{
	
			SetRedLEDStatus(5000);
       ch=huart4.Instance->DR;//get DR data and DR can auto clear
			switch(data_count)//֡ͷʇ0x0d 0x0a֡βʇ·´¹ýÀ´
			{
				case 0:
					if(ch==0x0D)//Ő¶ϊǷ񒑾­¿ªʼ
						data_count++;
				else
					data_count=0;
				
				SetRedLEDStatus(3000);
				break;
				case 1:
					if(ch==0x0A)//ͬɏԙ¼Ӓ»´΅жύ
					{
						i=0;
						data_count++;
					}
					else if(ch==0x0D);
					else
						data_count=0;
					SetRedLEDStatus(2000);
					break;
				case 2://¿ªʼ´«ʤʽ¾ݍ
					posture.data[i]=ch;
					i++;
					if(i>=24)//ȴû´«ʤͪԲһֱԚ´«ʤ£¬´«ʤͪµąжύ
					{
						i=0;
						data_count++;
					}
					SetRedLEDStatus(1000);
					break;
				case 3:
					if(ch==0x0A)//Ő¶ϊǷ񽡊ø
						data_count++;
					else
						data_count=0;
					SetRedLEDStatus(500);
					break;
				case 4:
					if(ch==0x0D)//´«ʤͪ³ɍ
					{
					zangle= posture.ActVal[0];
					xangle= posture.ActVal[1];
					yangle=posture.ActVal[2];
					pos_x =posture.ActVal[3];
					pos_y =posture.ActVal[4];
					w_z =posture.ActVal[5];
						if(HAL_GetTick() - RunTimeTimer>100)
						{
							HAL_GPIO_TogglePin(RLED_GPIO_Port,RLED_Pin);
							RunTimeTimer = HAL_GetTick();
						}
			
					}
					data_count=0;
					break;
				default:
						data_count=0;
						break;
		}
	}
}
		
void Stract(char strDestination[],char strSource[],int num)//ƴ´՗ַûµĺ¯ʽ
{
	int i=0,j=0;
	while(strDestination[i]!='\0') i++;//ˣ³ö֮ǰµĺ¯ʽ³¤¶ȍ
	for(j=0;j<num;j++)//ƴ½Ӕں󃦍
		strDestination[i++]=strSource[j];
	
}

void Update_X(float New_X)
{
	int i=0;
	char Update_x[8]="ACTX";
	static union
	{
		float X;
		char data[4];
	}New_set;
	New_set.X=New_X;//ʽ¾ݷŽøNew_set.X¡X
	Stract(Update_x,New_set.data,4);
	for(i=0;i<8;i++)//·¢ˍʽ¾ݍ
	{
		while(__HAL_UART_GET_FLAG(&huart4, UART_FLAG_RXNE)==RESET);
		HAL_UART_Transmit(&huart4, (uint8_t *)&Update_x[i], 8, 0x32);
	}
}
void Update_Y(float New_Y)
{
	int i=0;
	char Update_y[8]="ACTY";
	static union
	{
		float Y;
		char data[4];
	}New_set;
	New_set.Y=New_Y;
	Stract(Update_y,New_set.data,4);
	for(i=0;i<8;i++)
	{
		while(__HAL_UART_GET_FLAG(&huart4, UART_FLAG_RXNE)==RESET);
		HAL_UART_Transmit(&huart4, (uint8_t *)&Update_y[i], 8, 0x32);
	}
}

void Update_A(float New_A)//´ˎª½Ƕȍ
{
	int i=0;
	char Update_A[8]="ACTJ";
	static union
	{
		float A;
		char data[4];
	}New_set;
	New_set.A=New_A;
	Stract(Update_A,New_set.data,4);
	for(i=0;i<8;i++)
	{
		while(__HAL_UART_GET_FLAG(&huart4, UART_FLAG_RXNE)==RESET);
		HAL_UART_Transmit(&huart4, (uint8_t *)&Update_A[i], 8, 0x32);
	}
}
